#include <rclcpp/rclcpp.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
// #include <action_msgs/msg/goal_status.hpp>
// #include <tf2_ros/transform_listener.h>
// #include <tf2_ros/buffer.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <cmath>
#include "rclcpp_action/rclcpp_action.hpp"

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;

class NavToPoseClient : public rclcpp::Node
{
public:
    NavToPoseClient() : Node("demo_nav_to_target")
    {
         // 从参数服务器获取目标点坐标
        double x = this->declare_parameter<double>("x", 0.0);
        double y = this->declare_parameter<double>("y", 0.0);
        double yaw = this->declare_parameter<double>("yaw", 0.0);
        RCLCPP_INFO(this->get_logger(), "Sending goal to navigate to (%.2f, %.2f,%.2f)",
                    x, y,yaw);

        // 创建导航客户端
        navigate_to_pose_client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");

        // 等待导航服务启动
        while (!navigate_to_pose_client_->wait_for_action_server(std::chrono::seconds(1))) {
            RCLCPP_INFO(this->get_logger(), "Waiting for the navigate_to_pose action server...");
        }


        point_target_.position.x = x;  // B点x坐标
        point_target_.position.y = y;  // B点y坐标
        point_target_.orientation.w = cos(yaw/2.0); // B点方向（四元数）
        point_target_.orientation.z = sin(yaw/2.0);

        // 发送导航请求
        send_goal(point_target_);
    }

private:
    void send_goal(const geometry_msgs::msg::Pose &point_target)
    {
        auto goal_msg = NavigateToPose::Goal();
        goal_msg.pose.header.frame_id = "map";
        goal_msg.pose.header.stamp = this->now();
        goal_msg.pose.pose = point_target;

        auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        
        send_goal_options.goal_response_callback = std::bind(&NavToPoseClient::goal_response_callback, this, std::placeholders::_1);
        
        send_goal_options.feedback_callback = std::bind(&NavToPoseClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        send_goal_options.result_callback = std::bind(&NavToPoseClient::result_callback, this, std::placeholders::_1);

        navigate_to_pose_client_->async_send_goal(goal_msg, send_goal_options);
    }

    void goal_response_callback(GoalHandleNavigateToPose::SharedPtr goal_handle)
    {
        if (!goal_handle) {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        } else {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    void feedback_callback(GoalHandleNavigateToPose::SharedPtr, const std::shared_ptr<const NavigateToPose::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Distance remaining: %.2f m",feedback->distance_remaining);
    }

    void result_callback(const GoalHandleNavigateToPose::WrappedResult &result)
    {
        switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                RCLCPP_INFO(this->get_logger(), "导航结束 Goal succeeded!");
                break;
            case rclcpp_action::ResultCode::ABORTED:
                RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
                break;
            case rclcpp_action::ResultCode::CANCELED:
                RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
                break;
            default:
                RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                break;
        }
    }

    rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_to_pose_client_;
    geometry_msgs::msg::Pose point_target_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<NavToPoseClient>());
    rclcpp::shutdown();
    return 0;
}